/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "stdafx.h"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include <omp.h>
#include <iostream>


namespace cv
{

	static void
		calcHarris(const Mat& _cov, Mat& _dst, double k)
	{
			int i, j;
			Size size = _cov.size();
//#if CV_SSE
			volatile bool simd = checkHardwareSupport(CV_CPU_SSE);
//#endif

			if (_cov.isContinuous() && _dst.isContinuous())
			{
				size.width *= size.height;
				size.height = 1;
			}

			for (i = 0; i < size.height; i++)
			{
				const float* cov = (const float*)(_cov.data + _cov.step*i);
				float* dst = (float*)(_dst.data + _dst.step*i);
				j = 0;

//#if CV_SSE
				if (simd)
				{
					__m128 k4 = _mm_set1_ps((float)k);
					for (; j <= size.width - 5; j += 4)
					{
						__m128 t0 = _mm_loadu_ps(cov + j * 3); // a0 b0 c0 x
						__m128 t1 = _mm_loadu_ps(cov + j * 3 + 3); // a1 b1 c1 x
						__m128 t2 = _mm_loadu_ps(cov + j * 3 + 6); // a2 b2 c2 x
						__m128 t3 = _mm_loadu_ps(cov + j * 3 + 9); // a3 b3 c3 x
						__m128 a, b, c, t;
						t = _mm_unpacklo_ps(t0, t1); // a0 a1 b0 b1
						c = _mm_unpackhi_ps(t0, t1); // c0 c1 x x
						b = _mm_unpacklo_ps(t2, t3); // a2 a3 b2 b3
						c = _mm_movelh_ps(c, _mm_unpackhi_ps(t2, t3)); // c0 c1 c2 c3
						a = _mm_movelh_ps(t, b);
						b = _mm_movehl_ps(b, t);
						t = _mm_add_ps(a, c);
						a = _mm_sub_ps(_mm_mul_ps(a, c), _mm_mul_ps(b, b));
						t = _mm_mul_ps(_mm_mul_ps(k4, t), t);
						a = _mm_sub_ps(a, t);
						_mm_storeu_ps(dst + j, a);
					}
				}
//#endif

				for (; j < size.width; j++)
				{
					float a = cov[j * 3];
					float b = cov[j * 3 + 1];
					float c = cov[j * 3 + 2];
					dst[j] = (float)(a*c - b*b - k*(a + c)*(a + c));
				}
			}
		}




	void cornersModified(const Mat& src, Mat& eigenv, int block_size,
		int aperture_size, Mat& Dx, Mat& Dy, double k = 0., int borderType = cv::BORDER_REPLICATE)
	{
#ifdef HAVE_TEGRA_OPTIMIZATION
			if (tegra::cornerEigenValsVecs(src, eigenv, block_size, aperture_size, op_type, k, borderType))
				return;
#endif

			int depth = src.depth();
			double scale = (double)(1 << ((aperture_size > 0 ? aperture_size : 3) - 1)) * block_size;
			if (aperture_size < 0)
				scale *= 2.;
			if (depth == CV_8U)
				scale *= 255.;
			scale = 1. / scale;

			CV_Assert(src.type() == CV_8UC1 || src.type() == CV_32FC1);

			if (aperture_size <= 0)
			{
				CV_Error(CV_StsBadFlag, "aperture_size");
			}


			Size size = src.size();
			Mat cov(size, CV_32FC3);
			int i, j;

			for (i = 0; i < size.height; i++)
			{
				float* cov_data = (float*)(cov.data + i*cov.step);
				const float* dxdata = (const float*)(Dx.data + i*Dx.step);
				const float* dydata = (const float*)(Dy.data + i*Dy.step);

				for (j = 0; j < size.width; j++)
				{
					float dx = dxdata[j];
					float dy = dydata[j];

					cov_data[j * 3] = dx*dx;
					cov_data[j * 3 + 1] = dx*dy;
					cov_data[j * 3 + 2] = dy*dy;
				}
			}
			boxFilter(cov, cov, cov.depth(), Size(block_size, block_size),Point(-1, -1), false, borderType);
			calcHarris(cov, eigenv, k);

		}

}


